
function Main_DROShooting
%
% 三体模型下DRO轨道打靶
%
% 作者：张晨
% 邮箱：chenzhang@csu.ac.cn
% 单位：中国科学院空间应用工程与技术中心，空间探索室
% 时间：2022年02月24日
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc; clear; close all;

% 添加模块
parentFolder = fileparts(pwd);
addpath(genpath([parentFolder , '\Basic']));
addpath(genpath([parentFolder , '\Bcr4bp']));
addpath(genpath([parentFolder , '\Cr3bp']));

% 载入参数
aux = Bcr4bp_Aux;

mu = aux.EMRot.mu;
l_star = aux.dim.EMRot_l;
t_star = aux.dim.EMRot_t;
v_star = aux.dim.EMRot_v;

global xx0

% DRO状态猜测
x0_guess = 7e4; % km
vy0_guess = -0.5; % km/s
xx0 = [x0_guess / l_star ; 0 ; 0 ; vy0_guess / v_star] + [1 - mu ; 0 ; 0 ; 0];

% 画DRO初值
[ttTemp , xxTemp] = Cr3bp_Orbit2D([0 , pi] , xx0 , aux);

% 使用fsolve函数求解非线性方程组
options = optimoptions('fsolve', 'Display', 'iter');
[vy0 , fval] = fsolve(@myfun, vy0_guess, options , aux);

% 结果
fprintf('\n 修正后vy0为: % 0.4f km/s \n' , vy0)

% 画图
[F , tt , xx] = myfun(vy0 , aux);
Cr3bp_ScnEMRot(aux);
plotOrb2D(xxTemp , 'r' , 2); % DRO初值
plotOrb2D(xx , 'b' , 2); % DRO打靶收敛解
plotOrb2D(xx(1 , :) , 'rx' , 2);


%% 计算月心DRO相位角

% 计算相位角
xxTemp = [xx(: , 1) - (1 - mu) , xx(: , 2) , xx(: , 3) , xx(: , 4)];
theta = zeros(size(xx , 1) , 1);
xDim = zeros(size(xx , 1) , 1);
yDim = zeros(size(xx , 1) , 1);
tofDim = zeros(size(xx , 1) , 1);

% 循环DRO状态
for iLoop = 1 : size(xxTemp , 1)

    x = xxTemp(iLoop , 1);
    y = xxTemp(iLoop , 2);
    vx = xxTemp(iLoop , 3);
    vy = xxTemp(iLoop , 4);

    % km , km/s
    xxDim(iLoop , :) = [x * l_star , y * l_star , vx * v_star , vy * v_star];

    % day
    tofDim(iLoop) = tt(iLoop , 1) * t_star / 86400;

    % deg
    if iLoop == 1
        theta(iLoop) = rad2deg(2 * pi);
    else
        theta(iLoop) = rad2deg(atan3(y , x));
    end

end

% 保存DRO结果
DRO_dim = [];
DRO_dim.xxDim = xxDim;
DRO_dim.tofDim = tofDim;
DRO_dim.theta = theta;
save DRO_dim DRO_dim

% figure(2); grid on; axis equal;
% plotOrb2D(xxDim , 'b' , 2);

end



function [F, tt , xx] = myfun(vy0 , aux)
%
% 构造1x1打靶约束
% 打靶变量：vy0
% 打靶约束：绕1圈再次穿越X-Z平面的vx=0
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global xx0

mu = aux.EMRot.mu;
l_star = aux.dim.EMRot_l;
t_star = aux.dim.EMRot_t;
v_star = aux.dim.EMRot_v;

% 飞行时间上限
tof = 60 * 86400 / t_star;

% 速度归一化
xx0(4) = vy0 / v_star;

tt = [];
xx = [];
tau0Temp = 0;
xx0Temp = xx0;

% 绕N圈
N = 1;
for iLoop = 1 : N

    options = odeset('Reltol', 1e-8, 'AbsTol', 1e-8 , 'Events' , @Cr3bp_EventXZCross);
    [ttTemp , xxTemp] = ode113(@Cr3bp_Eqm2D , [tau0Temp , tau0Temp + tof] , xx0Temp , options , mu);

    tau0Temp = ttTemp(end);
    xx0Temp = xxTemp(end , :)';

    tt = [tt ; ttTemp];
    xx = [xx ; xxTemp];

end

% vx=0
F = xx(end , 3);

end
